/***********************************************************************************************
Copyright (c) Shenzhen Zhaowei Machinery & Electronics Co., Ltd. 2020-2030. All rights reserved.
************************************************************************************************
Filename:   smo.c
Brief:      The calculation of motor electrical angle base on SMO algorithm.
Author:     yangriyang
Date:       2023.02.09
Others:
History:
1   Date:
    Author: 
    Modification:   
2   Date:
    Author:
    Modification: 
3   Date:
    Author:
    Modification:
***********************************************************************************************/

#include "esmo.h"

#define FILTER_COE_SMO_POS 1                                        /* 低通滤波器滤波系数   a */
#define FILTER_COE_SMO_NEG 0                                        /* 低通滤波器滤波系数 1-a */

#define SMO_SAT_VAL         (2*65536)                               /* 饱和函数阈值设置为2 放大2^16 */
#define SMO_K 			    (25)		                            /* 滑膜增益 */


#define VALUE_2PI           (2048)                                  /* 2*PI */
#define VALUE_PI_DIV2       (512)                                   /* pi/2 */
#define PLL_KP              (35)                                    /* 锁相环比例参数 */
#define PLL_KI              (100)                                   /* 锁相环积分参数 */
#define SMO_TS              (2097LL)                                /* 积分时间 放大2^25 */

#define RS				    (462LL)				                    /* 相电阻<<12bit 0.094*1.2Ω*/
#define TI_DIV_LS 		    (5436LL)			                    /* 积分时间/电感 <<14bit 0.0000625s/0.000205H*/

#define DEGREE_2PI          (411775)
#define COE_2PI_TO_65535    (5215LL)

#define MIN_ELEC_SPEED      (655360)                                /* 最小电转速 10rad/s */

/***********************************************************************************************
Function:	EMF_Observer()
Brief:		观测反电动势
Input:      相对地电压、ab轴电流、电机参数、滤波系数、滑膜增益
Output:
Return:
Others: 	Ealpha、Ebeta应小于256V 防止数据溢出
***********************************************************************************************/
static void SmoObserver(SMO_T *smo)
{
    smo->uAlpha = smo->adcValuePhaseToGnd[0]<<-smo->adcValuePhaseToGnd[1]-smo->adcValuePhaseToGnd[2];
    smo->uBeta = smo->adcValuePhaseToGnd[1]-smo->adcValuePhaseToGnd[2];

    smo->iAlphaErr = smo->iAlphaEst - smo->iAlpha;
    smo->iBetaErr = smo->iBetaEst - smo->iBeta;

    if(smo->iAlphaErr > SMO_SAT_VAL)
    {
        smo->iAlphaErr = SMO_SAT_VAL;
    }
    else if(smo->iAlphaErr < -SMO_SAT_VAL)
    {
        smo->iAlphaErr = -SMO_SAT_VAL;
    }
    smo->eAlpha = ((FILTER_COE_SMO_POS * smo->eAlpha)>>8) + (FILTER_COE_SMO_NEG*(smo->iAlphaErr-smo->eAlpha)>>8);
    
    if(smo->iBetaErr > SMO_SAT_VAL)
    {
        smo->iBetaErr = SMO_SAT_VAL;
    }
    else if(smo->iBetaErr < -SMO_SAT_VAL)
    {
        smo->iBetaErr = -SMO_SAT_VAL;
    }
    smo->eBeta = ((FILTER_COE_SMO_POS * smo->eBeta)>>8) + (FILTER_COE_SMO_NEG*(smo->iBetaErr-smo->eBeta)>>8);

    smo->iAlpha = smo->iAlphaEst + ((int32_t)((((smo->eAlpha - ((smo->iAlphaEst * RS) >> 12)) - (smo->eAlpha)) * TI_DIV_LS) >> 14));
    smo->iBeta = smo->iBetaEst + ((int32_t)((((smo->eBeta - ((smo->iBetaEst * RS) >> 12)) - (smo->eBeta)) * TI_DIV_LS) >> 14));
}
/***********************************************************************************************
Function:	PLLObserver()
Brief:		速度观测+锁相环
Input:
Output:
Return:
Others: 	
***********************************************************************************************/
static void PLLObserver(SMO_T *smo)
{
    int32_t emfErr = 0;
    int32_t cosValue,sinValue;

    emfErr = ((-smo->eAlpha * cosValue)>>16) - ((smo->eBeta*sinValue)>>16);
    emfErr = PLL_KP * emfErr;
    smo->we += (emfErr * PLL_KI * SMO_TS)>>25;

}
/***********************************************************************************************
Function:	SmoThetaCal()
Brief:		SMO角度积分运算
Input:
Output:
Return:
Others: 	
***********************************************************************************************/
static void SmoThetaCal(SMO_T *smo)
{
    int32_t thetaRad, thetaPU;
    int32_t we;
    /* 电机运行时 为防止电机反转 需要设置最小的we */
    if(smo->we < MIN_ELEC_SPEED)
    {
        we = MIN_ELEC_SPEED;
    }
    else
    {
        we = smo->we;
    }

    thetaRad = smo->thetaRad + ((we * SMO_TS)>>25);
    if(thetaRad >= DEGREE_2PI)
    {
        thetaRad -= DEGREE_2PI;
    }
    else
    {
        thetaRad += DEGREE_2PI;
    }

    smo->thetaRad = thetaRad;
    thetaPU = (thetaRad * COE_2PI_TO_65535)>>15;
    smo->theta = thetaPU;
}
/***********************************************************************************************
Function:   SmoThetaSolve()
Brief:      无感角度解算  
Input:
Output:		
Return:     
Others:		
***********************************************************************************************/
void RunSmoThetaSolve(SMO_T *smo)
{
    int32_t uAlphaJudge, uBetaJudge;
    SmoObserver(smo);
    PLLObserver(smo);
    SmoThetaCal(smo);
    
    if(smo->eAlpha < 0)
    {
        uAlphaJudge = -smo->eAlpha;
    }
    else
    {
        uAlphaJudge = smo->eAlpha;
    }

    if(smo->eBeta < 0)
    {
        uBetaJudge = -smo->eBeta;
    }
    else
    {
        uBetaJudge = smo->eBeta;
    }

    if(smo->eAlpha < 3000 && smo->eBeta < 3000)
    {
        smo->we = 0;
        smo->eAlpha = 0;
        smo->eBeta = 0;
        smo->iAlphaEst = 0;
        smo->iBetaEst = 0;
    }
}
/***********************************************************************************************
Function:   ReadSmoSpeed()
Brief:	    读取滑模无感速度
Input:      
Output:                          
Return:
Others:		
***********************************************************************************************/
void ReadSmoSpeed(SMO_T *smo)
{
    return smo->we;
}
/***********************************************************************************************
Function:   ReadSmoTheta()
Brief:	    读取滑模无感速度
Input:      
Output:                          
Return:
Others:		
***********************************************************************************************/
void ReadSmoTheta(SMO_T *smo)
{
    return smo->theta;
}

